{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.6.9-final"
    },
    "colab": {
      "name": "Robotic Manipulation - Let's get you a robot.ipynb",
      "provenance": [],
      "collapsed_sections": [],
      "toc_visible": true
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "EgiF12Hf1Dhs",
        "colab_type": "text"
      },
      "source": [
        "**I recommend you run the first code cell of this notebook immediately, to start provisioning drake on the cloud machine, then you can leave this window open as you [read the textbook](manipulation.csail.mit.edu/robot.html).**\n",
        "\n",
        "# Notebook setup\n",
        "\n",
        "The following cell will:\n",
        "- on Colab (only), install Drake to `/opt/drake`, install Drake's prerequisites via `apt`, and add pydrake to `sys.path`.  This will take approximately two minutes on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  If you navigate between notebooks using Colab's \"File->Open\" menu, then you can avoid provisioning a separate machine for each notebook.\n",
        "- launch a server for our 3D visualizer (MeshCat) that will be used for the remainder of this notebook.\n",
        "\n",
        "You will need to rerun this cell if you restart the kernel, but it should be fast because the machine will already have drake installed."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "eeMrMI0-1Dhu",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "import importlib\n",
        "import sys\n",
        "from urllib.request import urlretrieve\n",
        "\n",
        "if 'google.colab' in sys.modules and importlib.util.find_spec('pydrake') is None:\n",
        "  version='20200918'\n",
        "  build='nightly'\n",
        "  urlretrieve(f\"https://drake-packages.csail.mit.edu/drake/{build}/drake-{version}/setup_drake_colab.py\",\n",
        "              \"setup_drake_colab.py\")\n",
        "  from setup_drake_colab import setup_drake\n",
        "  setup_drake(version=version, build=build)\n",
        "  !pip install pyngrok==4.2.2\n",
        "\n",
        "# Install pyngrok.\n",
        "server_args = []\n",
        "if 'google.colab' in sys.modules:\n",
        "  server_args = ['--ngrok_http_tunnel']\n",
        "\n",
        "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
        "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
        "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
        "\n",
        "# Determine if this notebook is currently running as a notebook or a unit test.\n",
        "from IPython import get_ipython\n",
        "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
        "\n",
        "# Imports\n",
        "import numpy as np\n",
        "import pydot\n",
        "from ipywidgets import Dropdown, Layout\n",
        "from IPython.display import display, HTML, SVG\n",
        "\n",
        "from pydrake.all import (\n",
        "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
        "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
        "    MultibodyPlant, Parser, Simulator)\n",
        "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "wJWL-ldv5REK",
        "colab_type": "text"
      },
      "source": [
        "# Robot arms\n",
        "\n",
        "The next two cells will give you a simple interface to move the joints around on a variety of robot arm models.\n",
        "\n",
        "Have a favorite robot that I haven't included here?  If you send me a link to a properly licensed URDF or SDF description of the robot and it's mesh files, I'm happy to add it!  It's worth taking a look at the files quickly, to get a sense for how they work: [SDF](https://github.com/RobotLocomotion/drake/blob/master/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf), [URDF](https://github.com/RobotLocomotion/drake/blob/master/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf).\n",
        "\n",
        "Note: The Jaco doesn't visualize properly in this renderer yet.  See drake issue [#13846](https://github.com/RobotLocomotion/drake/issues/13846)."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "wlN0Twwr5rYS",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "arm_selector = Dropdown(options=[\"Kuka LBR iiwa 7\",\n",
        "                        \"Kuka LBR iiwa 14\",\n",
        "                        \"Kinova Jaco Gen2 (7 DoF)\",\n",
        "                        \"Franka Emika Panda\"],\n",
        "               description=\"Choose your robot arm\",\n",
        "               style = {'description_width': 'initial'})\n",
        "display(arm_selector)\n",
        "\n",
        "def get_model_file(description):\n",
        "  # Note: I could download remote model resources here if necessary.\n",
        "  if description == \"Kuka LBR iiwa 7\":\n",
        "    return FindResourceOrThrow(\"drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf\")\n",
        "  elif description == \"Kuka LBR iiwa 14\":\n",
        "    return FindResourceOrThrow(\"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf\")\n",
        "  elif description == \"Kinova Jaco Gen2 (7 DoF)\":\n",
        "    return FindResourceOrThrow(\"drake/manipulation/models/jaco_description/urdf/j2s7s300.urdf\")\n",
        "  elif description == \"Franka Emika Panda\":\n",
        "    return FindResourceOrThrow(\"drake/manipulation/models/franka_description/urdf/panda_arm_hand.urdf\")\n",
        "  raise Exception(\"Unknown model\")\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "9GSF52A8ExQa",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "builder = DiagramBuilder()\n",
        "\n",
        "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)\n",
        "Parser(plant, scene_graph).AddModelFromFile(get_model_file(arm_selector.value))\n",
        "plant.Finalize()\n",
        "\n",
        "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
        "diagram = builder.Build()\n",
        "context = diagram.CreateDefaultContext()\n",
        "\n",
        "meshcat.load()\n",
        "MakeJointSlidersThatPublishOnCallback(plant, meshcat, context);"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "nPy2VgSH1Dh5",
        "colab_type": "text"
      },
      "source": [
        "# Simulation with block diagrams\n",
        "\n",
        "In this chapter, and throughout the notes, we will be building up our simulations and controllers using drake's block diagram modeling language (we call it the \"systems framework\").  [This tutorial](https://mybinder.org/v2/gh/RobotLocomotion/drake/nightly-release?filepath=tutorials/dynamical_systems.ipynb)  provides a brief introduction.  \n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Dl9NDxDR1Dh6",
        "colab_type": "text"
      },
      "source": [
        "# Simulating the (passive) iiwa\n",
        "\n",
        "Let's load our robot of choice (the Kuka iiwa) into the physics engine, which we call [MultibodyPlant](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html).\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "DyfpxsGR1Dh6",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "plant = MultibodyPlant(time_step=1e-4)\n",
        "Parser(plant).AddModelFromFile(\n",
        "        FindResourceOrThrow(\"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf\"))\n",
        "plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"iiwa_link_0\"))\n",
        "plant.Finalize()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "H3G-pTB5bit8",
        "colab_type": "text"
      },
      "source": [
        "In Drake, we are very careful to separate the *state* of a system from the system itself.  For example, let's think of a dynamical system as given by the difference equation: $$x[n+1] = f(n, x[n], u[n]),$$ where $n$ is the \"time\", $x$ is the state, and $u$ is any inputs to the system.  The `System` describes $f()$, but we use a structure called the [`Context`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_context.html) to hold the runtime values: $n, x, u$.  We use a structure, because some systems have even more runtime values, that describe system parameters, random noise inputs, etc, and it is cleaner to pass a structure around than have each system accept a different list of possible inputs.  As a rule `System` is constant over the duration of a simulation, and you should be able to obtain repeatable, deterministic simulations given any value `Context`.\n",
        "\n",
        "Let's see what the `Context` looks like for the physics engine now that we've loaded the iiwa into it."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "P8HoF2KPdXNL",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "context = plant.CreateDefaultContext()\n",
        "print(context)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "vr80vD3Be7uZ",
        "colab_type": "text"
      },
      "source": [
        "The system has 14 state variables -- these are the 7 joint positions and 7 joint velocities for the iiwa.  Those states are discrete, because we passed a non-zero `time_step` to the constructor of the `MultibodyPlant`, which means the physics engine will be use a time-stepping simulation scheme (this will be the most performant when we start simulating contact).  Go ahead and try changing `time_step` to zero, and see what happens.\n",
        "\n",
        "You'll notice the context has lots of parameters (currently these include the mass, the center of mass, and the inertia of each body).  You won't need those any time soon, but it does enable some super cool advanced features.\n",
        "\n",
        "Because the `Context` for a `MultibodyPlant` can get quite complicated, `MultibodyPlant` offers some methods that help you get/set the values.  Let's set some non-zero initial positions."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "73whw6YZf9PP",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "# Set all of the joint positions at once in a single vector.\n",
        "plant.SetPositions(context, [-1.57, 0.1, 0, 0, 0, 1.6, 0])\n",
        "# You can also set them by referencing particular joints.\n",
        "plant.GetJointByName(\"iiwa_joint_4\").set_angle(context, -1.2)\n",
        "print(context)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "LpQlGh1Ai27g",
        "colab_type": "text"
      },
      "source": [
        "The iiwa model also defined 7 actuators.  Before we can run a simulation, we need to provide some values for those inputs to the `MultibodyPlant` `actuation_input_port`.  For this warm-up, we'll just set them to zero."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "dJx-KrGUjR8x",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "plant.get_actuation_input_port().FixValue(context, np.zeros(7));"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "zqb66IBcierZ",
        "colab_type": "text"
      },
      "source": [
        "Now we can set up and run a simulation."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "6DYEtGbtimit",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "simulator = Simulator(plant, context)\n",
        "simulator.AdvanceTo(5.0)\n",
        "print(context)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "8ZKEl1QU1Dh9",
        "colab_type": "text"
      },
      "source": [
        "# Visualizing the scene\n",
        "\n",
        "The best way to visualize the results of a physics engine is with a 2D or 3D visualizer. For that, we need to add the system which curates the geometry of a scene (the geometry engine); in Drake with call it the [`SceneGraph`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1geometry_1_1_scene_graph.html). Once we have a `SceneGraph`, then there are a number of different visualizers and sensors that we can add to the system to actually render the scene.\n",
        "\n",
        "We support a number of visualizers in drake (we ship a simple VTK-based \"drake-visualizer\" with our binary releases, or you can use RViz).  But because we're working in a jupyter notebook, and want you to be able to run 3D graphics on any machine without any installation required, we'll use the [MeshCat](https://github.com/rdeits/meshcat) visualizer throughout these notes.  \n",
        "\n",
        "To use all three systems (`MultibodyPlant`, `SceneGraph`, and `MeshcatVisualizer`), we need to assemble them into a [`Diagram`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_diagram.html) using a [`DiagramBuilder`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_diagram_builder.html).  Often you would add each system and then connect the ports together, but for common cases like we have here, we provide helper methods to reduce the boilerplate."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "WkfjAgrL1Dh-",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "builder = DiagramBuilder()\n",
        "\n",
        "# Adds both MultibodyPlant and the SceneGraph, and wires them together.\n",
        "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)\n",
        "# Note that we parse into both the plant and the scene_graph here.\n",
        "Parser(plant, scene_graph).AddModelFromFile(\n",
        "        FindResourceOrThrow(\"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf\"))\n",
        "plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"iiwa_link_0\"))\n",
        "plant.Finalize()\n",
        "\n",
        "# Adds the MeshcatVisualizer and wires it to the SceneGraph.\n",
        "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
        "\n",
        "diagram = builder.Build()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Rm4kVRMS1DiF",
        "colab_type": "text"
      },
      "source": [
        "If you click on the link above, you can open a browser window to see the visualization.\n",
        "\n",
        "But the scene will be empty so far.  We need to actually construct the diagram and ask it to publish.  (You normally don't have to call these; MeshcatVisualizer will automatically load at initialization and publish on a fixed period during simulation)."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "2YMNZBi11DiN",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "context = diagram.CreateDefaultContext()\n",
        "meshcat.load()\n",
        "diagram.Publish(context)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "kPcyp3zv1Dir",
        "colab_type": "text"
      },
      "source": [
        "We've put together quite a complex system now.  A `Diagram` is just another `System`, so it has an associated context."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "Kr9BgEkcCQaZ",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "print(context)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "gu6_NRm_Cl-O",
        "colab_type": "text"
      },
      "source": [
        "You can see that there is one additional \"abstract state\" from the `SceneGraph`.  `MeshcatVisualizer` is stateless.  Actually `SceneGraph` should really be stateless for these simple examples, too [#9501](https://github.com/RobotLocomotion/drake/issues/9501).\n",
        "\n",
        "It's also very useful to draw the actual block diagram."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "XTeyZc7n1Dit",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString())[0].create_svg())"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "5OwrpdeGE5aa",
        "colab_type": "text"
      },
      "source": [
        "There is one more important detail: **The `Context` for the `Diagram` is not the `Context` of the `MultibodyPlant`.  But you can extract the plant `Context`; and we need to in order to set the initial conditions of the robot."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "aA5dasXyFWXo",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "plant_context = plant.GetMyMutableContextFromRoot(context)\n",
        "plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0])\n",
        "plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7))\n",
        "print(context)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Js2ulEaFF0yD",
        "colab_type": "text"
      },
      "source": [
        "Accidentally using `context` instead of `plant_context` is a very common mistake!\n",
        "\n",
        "Ok, now we're ready to simulate.  Make sure your visualizer window is visible, then run the following cell."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "ZySlXAKSGLaS",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "simulator = Simulator(diagram, context)\n",
        "simulator.set_target_realtime_rate(1.0)\n",
        "simulator.AdvanceTo(5.0 if running_as_notebook else 0.1)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Fdkx17YxGPSz",
        "colab_type": "text"
      },
      "source": [
        "Glorious!\n",
        "\n",
        "Here's one more trick.  You can ask MeshCat to record the animation, and then review it in the visualizer.  I'll repeat the preamble here to make this cell stand-alone.  (Plus we cannot reuse a system in multiple diagrams; the ownership is restricted)."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "HUeu3reLWw_H",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "builder = DiagramBuilder()\n",
        "\n",
        "# Adds both MultibodyPlant and the SceneGraph, and wires them together.\n",
        "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)\n",
        "# Note that we parse into both the plant and the scene_graph here.\n",
        "Parser(plant, scene_graph).AddModelFromFile(\n",
        "        FindResourceOrThrow(\"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf\"))\n",
        "plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"iiwa_link_0\"))\n",
        "plant.Finalize()\n",
        "\n",
        "# Adds the MeshcatVisualizer and wires it to the SceneGraph.\n",
        "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, delete_prefix_on_load=False)\n",
        "\n",
        "diagram = builder.Build()\n",
        "context = diagram.CreateDefaultContext()\n",
        "plant_context = plant.GetMyMutableContextFromRoot(context)\n",
        "plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0])\n",
        "plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7))\n",
        "\n",
        "simulator = Simulator(diagram, context)\n",
        "simulator.set_target_realtime_rate(1.0)\n",
        "\n",
        "meshcat.start_recording()\n",
        "simulator.AdvanceTo(5.0 if running_as_notebook else 0.1)\n",
        "meshcat.stop_recording()\n",
        "meshcat.publish_recording()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "8SPr8dXoY3IT",
        "colab_type": "text"
      },
      "source": [
        "# Adding the iiwa controller\n",
        "\n",
        "The iiwa hardware interface runs through a control system provided by the manufacturer.  We can't turn it off.  The best we can do is add our approximation of it to our simulation.  I'll repeat everything we've set up before (to keep this example self-contained), but we'll add one more system for the controller."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "Qd245P5kY666",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "builder = DiagramBuilder()\n",
        "\n",
        "# Adds both MultibodyPlant and the SceneGraph, and wires them together.\n",
        "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)\n",
        "# Note that we parse into both the plant and the scene_graph here.\n",
        "iiwa_model = Parser(plant, scene_graph).AddModelFromFile(\n",
        "        FindResourceOrThrow(\"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf\"))\n",
        "plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"iiwa_link_0\"))\n",
        "plant.Finalize()\n",
        "\n",
        "# Adds the MeshcatVisualizer and wires it to the SceneGraph.\n",
        "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, delete_prefix_on_load=False)\n",
        "\n",
        "# Adds an approximation of the iiwa controller.\n",
        "# TODO(russt): replace this with the joint impedance controller.\n",
        "Kp = np.full(7, 100)\n",
        "Ki = 2 * np.sqrt(Kp)\n",
        "Kd = np.full(7, 1)\n",
        "iiwa_controller = builder.AddSystem(InverseDynamicsController(plant, Kp, Ki, Kd, False))\n",
        "iiwa_controller.set_name(\"iiwa_controller\");\n",
        "builder.Connect(plant.get_state_output_port(iiwa_model),\n",
        "                iiwa_controller.get_input_port_estimated_state())\n",
        "builder.Connect(iiwa_controller.get_output_port_control(),\n",
        "                plant.get_actuation_input_port())\n",
        "diagram = builder.Build()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "SwmwcTcOX-pm",
        "colab_type": "text"
      },
      "source": [
        "Let's see what our diagram looks like..."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "1SMxPMvdxk99",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString())[0].create_svg())"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ZWoEJzsixwGj",
        "colab_type": "text"
      },
      "source": [
        "As our diagrams get more complex, we're going to need a better approach to rendering the diagrams.  I've got a rough first pass (that it still suffering from some licensing issues) in javascript that I aim to improve.\n",
        "\n",
        "TODO(russt): Update the javascript rendering [#13874](https://github.com/RobotLocomotion/drake/issues/13874).  "
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "EjJzh5veYA6e",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "diagram.set_name(\"diagram\")\n",
        "HTML('<script src=\"https://unpkg.com/gojs/release/go.js\"></script>' + GenerateHtml(diagram))"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "wuo1q8PgXzrp",
        "colab_type": "text"
      },
      "source": [
        "To set up the `Context` now, instead of setting the `actuation_input` to zero, we set the `desired_state` of the iiwa controller to be the current position of the arm.  But the desired *state* must also include desired velocities, so we set those to zero."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "GHrnvvpXXixB",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "context = diagram.CreateDefaultContext()\n",
        "plant_context = plant.GetMyMutableContextFromRoot(context)\n",
        "q0 = np.array([-1.57, 0.1, 0, -1.2, 0, 1.6, 0])\n",
        "x0 = np.hstack((q0, 0*q0))\n",
        "plant.SetPositions(plant_context, q0)\n",
        "iiwa_controller.GetInputPort('desired_state').FixValue(\n",
        "    iiwa_controller.GetMyMutableContextFromRoot(context), x0)\n",
        "print(context)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "kAWFX5H7atro",
        "colab_type": "text"
      },
      "source": [
        "Now, if we simulate, the robot just stays put.  Just like the real robot would."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "qvJ2PptyYuz2",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "simulator = Simulator(diagram, context)\n",
        "simulator.set_target_realtime_rate(1.0);\n",
        "simulator.AdvanceTo(5.0 if running_as_notebook else 0.1);"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "7q0A14bAilIX",
        "colab_type": "text"
      },
      "source": [
        "# Robot hands\n",
        "\n",
        "I don't have the same smörgåsbord of robot models to offer for robot hands (yet).  We do have the allegro hand model available, and I'm happy to add more here (though many will require some care to handle their kinematic constraints).  For now, you can at least try out the [Allegro Hand](http://www.wonikrobotics.com/Allegro-Hand.htm).\n",
        "\n",
        "TODO(russt): Consider porting Robotiq, Sandia, or IHY-hand from our [openhumanoids](https://github.com/openhumanoids/oh-distro/tree/master/software/models/common_components) project."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "ILYLouFTjv6e",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "def hand_sliders():\n",
        "    builder = DiagramBuilder()\n",
        "\n",
        "    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)\n",
        "    allegro_file = FindResourceOrThrow(\"drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf\")\n",
        "    Parser(plant, scene_graph).AddModelFromFile(allegro_file)\n",
        "    plant.Finalize()\n",
        "\n",
        "    meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "\n",
        "    meshcat.load()\n",
        "    MakeJointSlidersThatPublishOnCallback(plant, meshcat, context)\n",
        "\n",
        "hand_sliders()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "5SjOClhTltPk",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        ""
      ],
      "execution_count": null,
      "outputs": []
    }
  ]
}